#include "../Include/WizGeometryIntersection.hpp"

namespace Wiz
{
    //////////////////////////////////////////////////////////////////////////
    /// Ray ^ Plane
    template<class ElementT>
    ::Wiz::Bool::Type Intersection<ElementT>::operator()(const tRay& InRay, const tPlane& InPlane)
    {
        tElement t = tVector3::Dot(InRay.GetDirection(), InPlane.GetNormal());
        if (::Wiz::IsZero(t))
        {
            return InPlane.IsInPlane(InRay.GetOrigin());
        }

        return ::Wiz::Bool::True;
    }

    /// OutPnt : Intersection Point
    template<class ElementT>
    ::Wiz::Bool::Type Intersection<ElementT>::operator()(tVector3& OutPnt, const tRay& InRay, const tPlane& InPlane)
    {
        tElement Denominator = tVector3::Dot(InRay.GetDirection(), InPlane.GetNormal());
        if (::Wiz::IsZero(Denominator))
        {
            if (InPlane.IsInPlane(InRay.GetOrigin()))
            {
                OutPnt = InRay.GetOrigin();
                return ::Wiz::Bool::True;
            }
            else
            {
                /// So parallel
                return ::Wiz::Bool::False;
            }
        }

        tElement t = -(tVector3::Dot(InRay.GetOrigin(), InPlane.GetNormal()) + InPlane.GetConstant());
        t /= Denominator;

        if (::Wiz::GreaterEqualZero(t))
        {
            OutPnt = InRay.GetOrigin() + t * InRay.GetDirection();
            return ::Wiz::Bool::True;
        }
        return ::Wiz::Bool::False;
    }

    /// Ray ^ Sphere
    template<class ElementT>
    ::Wiz::Bool::Type Intersection<ElementT>::operator()(const tRay& InRay, const tSphere& InSphere)
    {
        const tVector3& RayDir = InRay.GetDirection();
        // Adjust ray origin relative to sphere center
        const tVector3 RayOrig = InRay.GetOrigin() - InSphere.GetCenter();
        const tElement RadiusPow2 = InSphere.GetRadius() * InSphere.GetRadius();

        if (RayOrig.SqrLength() < RadiusPow2)
        {
            return ::Wiz::Bool::True;
        }

        const tElement a = tVector3::Dot(RayDir, RayDir);
        const tElement b = 2 * tVector3::Dot(RayOrig, RayDir);
        const tElement c = tVector3::Dot(RayOrig, RayOrig) - RadiusPow2;

        if ((b * b - 4 * a * c) < 0)
        {
            return ::Wiz::Bool::False;
        }

        return ::Wiz::Bool::True;
    }


    //////////////////////////////////////////////////////////////////////////
    /// Ray ^ Sphere
    template<class ElementT>
    ::Wiz::Int::Type Intersection<ElementT>::operator()(tVector3& OutPnt1, tVector3& OutPnt2, const tRay& InRay, const tSphere& InSphere)
    {
        const tVector3& RayDir = InRay.GetDirection();
        // Adjust ray origin relative to sphere center
        const tVector3 RayOrig = InRay.GetOrigin() - InSphere.GetCenter();
        const tElement RadiusPow2 = InSphere.GetRadius() * InSphere.GetRadius();

        const tElement a = tVector3::Dot(RayDir, RayDir);
        const tElement b = 2 * tVector3::Dot(RayOrig, RayDir);
        const tElement c = tVector3::Dot(RayOrig, RayOrig) - RadiusPow2;

        tElement t1, t2;
        const ::Wiz::Int::Type i = ::Wiz::QuadraticEquationRoots(t1, t2, a, b, c);
        if (i == 0)
        {
            return 0;
        }

        ::Wiz::Int::Type cnt = 0;
        if (t1 >= 0)
        {
            OutPnt1 = InRay.GetPoint(t1);
            cnt++;
        }

        if ((i == 2) && (t2 >= 0))
        {
            if (cnt == 0)
            {
                OutPnt1 = InRay.GetPoint(t2);
            }
            else
            {
                OutPnt2 = InRay.GetPoint(t2);
            }
            cnt++;
        }

        return cnt;
    }

    template<class ElementT>
    ::Wiz::Bool::Type Intersection<ElementT>::operator()(const tRay& InRay, const tAABB& InAABB)
    {
        tElement t;
        tVector3 HitPnt;
        const tVector3& AABBMin = InAABB.GetMin();
        const tVector3& AABBMax = InAABB.GetMax();
        const tVector3& RayOrig = InRay.GetOrigin();
        const tVector3& RayDir = InRay.GetDirection();

        if (InAABB.IsInclude(RayOrig))
        {
            return ::Wiz::Bool::True;
        }

        // Check each face in turn, only check closest 3
        // Min x
        if (RayOrig.x <= AABBMin.x && RayDir.x > 0)
        {
            t = (AABBMin.x - RayOrig.x) / RayDir.x;
            if (t >= 0)
            {
                // Substitute t back into ray and check bounds and dist
                HitPnt = RayOrig + RayDir * t;
                if (HitPnt.y >= AABBMin.y && HitPnt.y <= AABBMax.y &&
                    HitPnt.z >= AABBMin.z && HitPnt.z <= AABBMax.z)
                {
                    return ::Wiz::Bool::True;
                }
            }
        }

        // Max x
        if (RayOrig.x >= AABBMax.x && RayDir.x < 0)
        {
            t = (AABBMax.x - RayOrig.x) / RayDir.x;
            if (t >= 0)
            {
                // Substitute t back into ray and check bounds and dist
                HitPnt = RayOrig + RayDir * t;
                if (HitPnt.y >= AABBMin.y && HitPnt.y <= AABBMax.y &&
                    HitPnt.z >= AABBMin.z && HitPnt.z <= AABBMax.z)
                {
                    return ::Wiz::Bool::True;
                }
            }
        }
        // Min y
        if (RayOrig.y <= AABBMin.y && RayDir.y > 0)
        {
            t = (AABBMin.y - RayOrig.y) / RayDir.y;
            if (t >= 0)
            {
                // Substitute t back into ray and check bounds and dist
                HitPnt = RayOrig + RayDir * t;
                if (HitPnt.x >= AABBMin.x && HitPnt.x <= AABBMax.x &&
                    HitPnt.z >= AABBMin.z && HitPnt.z <= AABBMax.z)
                {
                    return ::Wiz::Bool::True;
                }
            }
        }
        // Max y
        if (RayOrig.y >= AABBMax.y && RayDir.y < 0)
        {
            t = (AABBMax.y - RayOrig.y) / RayDir.y;
            if (t >= 0)
            {
                // Substitute t back into ray and check bounds and dist
                HitPnt = RayOrig + RayDir * t;
                if (HitPnt.x >= AABBMin.x && HitPnt.x <= AABBMax.x &&
                    HitPnt.z >= AABBMin.z && HitPnt.z <= AABBMax.z)
                {
                    return ::Wiz::Bool::True;
                }
            }
        }
        // Min z
        if (RayOrig.z <= AABBMin.z && RayDir.z > 0)
        {
            t = (AABBMin.z - RayOrig.z) / RayDir.z;
            if (t >= 0)
            {
                // Substitute t back into ray and check bounds and dist
                HitPnt = RayOrig + RayDir * t;
                if (HitPnt.x >= AABBMin.x && HitPnt.x <= AABBMax.x &&
                    HitPnt.y >= AABBMin.y && HitPnt.y <= AABBMax.y)
                {
                    return ::Wiz::Bool::True;
                }
            }
        }
        // Max z
        if (RayOrig.z >= AABBMax.z && RayDir.z < 0)
        {
            t = (AABBMax.z - RayOrig.z) / RayDir.z;
            if (t >= 0)
            {
                // Substitute t back into ray and check bounds and dist
                HitPnt = RayOrig + RayDir * t;
                if (HitPnt.x >= AABBMin.x && HitPnt.x <= AABBMax.x &&
                    HitPnt.y >= AABBMin.y && HitPnt.y <= AABBMax.y)
                {
                    return ::Wiz::Bool::True;
                }
            }
        }
        return ::Wiz::Bool::False;
    }

    template struct Intersection<::Wiz::Real32::Type>;
    template struct Intersection<::Wiz::Real64::Type>;
} /// end of namespace Wiz
